# -*- coding: utf-8 -*-
import time
from PyQt4 import QtCore, QtGui, uic
from mavlinkParser import mavlinkParser
import mavlinkDataManager

class mavlinkStyleBuffer(mavlinkParser):
    def __init__(self):
        mavlinkParser.__init__(self,10)

    def parseMsg(self):
        bufferLast = self.buffer
        validMsgs,abandonedCount = mavlinkParser.parseMsg(self)
        currentBufferLen =len(self.buffer)
        lastBufferLen  =len(bufferLast)
        if validMsgs is None: 
            assert(lastBufferLen-currentBufferLen>=0)
            sendStr =  bufferLast[0:(lastBufferLen-currentBufferLen)]
            if lastBufferLen-currentBufferLen !=0:
                return [sendStr]
            else:
                return []
        else:
            lastIndex = 0
            sendstrlist=[]
            for validMsg in validMsgs:
                if validMsg['startIndex'] != lastIndex:
                    sendstrlist.append(bufferLast[lastIndex:validMsg['startIndex']])
                lastIndex = validMsg['startIndex'] + validMsg['len']+8 
                msg = bufferLast[validMsg['startIndex']:lastIndex]
                sendstrlist.append(msg)
            
            if lastIndex+len(self.buffer)-1!= lastBufferLen:
                # print 'tail found'
                lastStr = bufferLast[lastIndex:(lastBufferLen-currentBufferLen)]
                sendstrlist.append(lastStr)
            return sendstrlist


class serialDataBuffer(QtCore.QThread):
    def __init__(self,device=None):
        QtCore.QThread.__init__(self)
        self.buffer = ''
        self.moveToThread(self)
        self.timer = QtCore.QTimer()
        self.timer.moveToThread(self)
        self.timer.timeout.connect(self.readData)

        # timer for read mirror serial
        self.mirrorTimer = QtCore.QTimer()
        self.mirrorTimer.moveToThread(self)
        self.mirrorTimer.timeout.connect(self.readMirrorData)

        self.__isRunning = False

        self.setDevice(device)
        self.BUFFER_MAX_LEN = 1e25
        self.warningDone = False
        self.mirrorDev = None
        self.__isbidirection = False
        self.__mavlinkStyle  = True
        self.__shouldForwarding = False
        
        self.__mavlinkBuffer = None

        # all of data received
        self.bufferTotal = ''
    def start(self,condition=True):
        self.__isRunning = True
        # self.timer.start()
        QtCore.QThread.start(self)
    
    @QtCore.pyqtSlot()
    def restart(self):
        self.__isRunning = True
        self.timer.start()
        if self.__shouldForwarding:
            self.mirrorTimer.start()

    @QtCore.pyqtSlot()
    def stop(self,condition=True):
        self.__isRunning = False
        # print "tst"
        # print 'timer stop'
        self.timer.stop()
        self.mirrorTimer.stop()
        self.clear()

    def readData(self):
        # print "i am reading data from port"
        if self.dev is not None and self.__isRunning:
            self.timer.stop()
            dataInWaitingSize=self.dev.inWaiting()
            data = self.dev.read(dataInWaitingSize)
            self.buffer = self.buffer + data
            self.bufferTotal = self.bufferTotal +data
            if len(self.bufferTotal)>self.BUFFER_MAX_LEN and  not self.warningDone:
                self.warningDone = True
                warning("buffer is too big ")
            if self.__shouldForwarding:
                self.mirrorDev.write(data)
            
            self.timer.start()
    
    def readMirrorData(self):
        if self.mirrorDev is not None and self.__shouldForwarding:
            self.mirrorTimer.timeout.disconnect(self.readMirrorData)
            dataInWaitingSize = self.mirrorDev.inWaiting()
            data = self.mirrorDev.read(dataInWaitingSize)

            if self.__mavlinkStyle:
                sendlist = self.__mavlinkBuffer.push(data)
                if type(sendlist)==list:
                    for senditem in sendlist:
                        self.dev.write(senditem)
                        time.sleep(0.001)
            else:
                self.dev.write(data)
            self.mirrorTimer.timeout.connect(self.readMirrorData)

    def run(self):
        self.__isRunning = True
        self.timer.setInterval(3)
        # print "tst"
        self.timer.start()
        # print "tstd"
        
        #start eventloop
        self.exec_()

    def clear(self):
        self.buffer=''
        self.clearTotalBuffer()
        if self.__mavlinkBuffer is not None:
            self.__mavlinkBuffer.clear()

    def clearTotalBuffer(self):
        self.bufferTotal=''

    def totalBufferLength(self):
        return len(self.bufferTotal)

    def setDevice(self,device):
        self.dev = device

    def inWaiting(self):
        return len(self.buffer)

    def dump(self):
        return self.bufferTotal

    def read(self,dataSize):
        # print "i am read"
        dataSize=min(dataSize,len(self.buffer))
        if self.__isRunning:
            # pass
            self.timer.timeout.disconnect(self.readData)
        outputdata = self.buffer[0:dataSize]
        self.buffer=self.buffer[dataSize:]
        if self.__isRunning:
            # pass
            self.timer.timeout.connect(self.readData)
        return outputdata

    def setMirrorDevice(self,device):
        self.mirrorDev = device

    @QtCore.pyqtSlot(object)
    def updateMirrorSetting(self,data):
        if data['disableMirror'] and self.mirrorDev is not None:
            self.mirrorDev.close()
            self.mirrorDev=None
            self.__shouldForwarding = False
            self.mirrorTimer.stop()
            self.__mavlinkBuffer = None 
        else:
            self.setMirrorDevice(data['mirrorDevice'])
            self.__isbidirection = data['bidirection']
            self.__mavlinkStyle  = data['mavlinkStyle']
            self.__shouldForwarding = True
            
            if self.__mavlinkStyle:
                self.__mavlinkBuffer=mavlinkStyleBuffer()

            self.mirrorTimer.setInterval(3)
            self.mirrorTimer.start()

class serialCon(QtCore.QThread):
    received = QtCore.pyqtSignal(object)
    speedupdate = QtCore.pyqtSignal(object)
    mavlinkParseUpdate = QtCore.pyqtSignal(object)
    mavlinkSpeedUpdate = QtCore.pyqtSignal(object)
    onStart  = QtCore.pyqtSignal(object)
    onStop   = QtCore.pyqtSignal(object)
    def __init__(self,device):
        QtCore.QThread.__init__(self)

        self.moveToThread(self)
        self.timer = QtCore.QTimer()
        self.timer.moveToThread(self)
        self.timer.setInterval(100)
        self.timer.timeout.connect(self.readData)


        self.bufferedserial=serialDataBuffer(device)
        self.onStart.connect(self.bufferedserial.restart)
        self.onStart.connect(self.startTimer)

        self.onStop.connect(self.bufferedserial.stop)
        self.onStop.connect(self.stopTimer)

        self.__isRunning = False

        self.timetag = 0
        self.totalData = 0
        self.dataReceiveLengthList = list()
        self.dataReceiveLengthListMaxLengh = 100
        self.dataReceiveTimeList  = list()
        
        self.lastupdateSpeedTime =0

        # swidth for parsing mavlink
        self.needParseMavlink = False
        self.mavlinkParser    = None
        self.abandonedCount   = 0
    def run(self):
        self.timer.setInterval(100)
        self.timer.start()
        self.__isRunning=True
        self.bufferedserial.start()
        #start eventloop
        self.exec_()
    

    def stop(self):
        # TODO read the buffer before stopping
        # print "i am stop"
        self.__isRunning = False
        self.totalData=0
        self.dataReceiveLengthList = list()
        self.dataReceiveTimeList  = list()

        # about serial
        self.onStop.emit(True)

    def start(self):
        self.timetag  =  time.time()
        self.__isRunning = True

        self.onStart.emit(True)
        QtCore.QThread.start(self)
    
    @QtCore.pyqtSlot()
    def stopTimer(self):
        self.timer.stop()

    @QtCore.pyqtSlot()
    def startTimer(self):
        self.timer.start()

    def restart(self):
        self.timetag  =  time.time()
        self.__isRunning = True
        self.onStart.emit(True)

    def setDevice(self,device):
        self.bufferedserial.setDevice(device)

    def clearStat(self):
        # clear statistic data
        self.totalData = 0 
        self.timetag = time.time()
        self.dataReceiveLengthList = list()
        self.dataReceiveTimeList  = list()
        self.lastupdateSpeedTime =0
        self.bufferedserial.clear()

    def readData(self):
        # print "i am reading data from buffer"
        mavlinkMsgs = None 
        dataInWaitingSize = 0 
        if self.__isRunning:
            #print  'i am reading'
            self.timer.stop()
            dataInWaitingSize=self.bufferedserial.inWaiting()
            if dataInWaitingSize !=0:
                data = self.bufferedserial.read(dataInWaitingSize)
                timeNow = time.time()
                self.received.emit(data)
                if self.needParseMavlink:
                    mavlinkMsgs,abandonedCount = self.mavlinkParser.push(data)
                    if(mavlinkMsgs is not None):
                        self.abandonedCount =self.abandonedCount +  abandonedCount
                        self.mavlinkParseUpdate.emit([mavlinkMsgs,self.abandonedCount])
                    self.timer.setInterval(100)
                else:   
                    self.timer.setInterval(40)
            else:
                timeNow = time.time()
                self.timer.setInterval(100)
            self.timer.start()
        else:
            self.timer.setInterval(1000)
        
        # speed measurement
        # the Instantaneous speed is the received data in the last second
        if self.__isRunning:
            self.dataReceiveLengthList.append(dataInWaitingSize)
            self.dataReceiveTimeList.append(timeNow)

            # find datalength in the last second
            lastIndex = len(self.dataReceiveLengthList)-1
            instSpeed = 0.0
            if lastIndex >0:
                timecurrent = self.dataReceiveTimeList[lastIndex]
                tIndex   = 0
                oneSecRec = self.dataReceiveLengthList[lastIndex]
                for i in range(lastIndex-1,-1,-1):
                    t  = self.dataReceiveTimeList[i]
                    oneSecRec = oneSecRec + self.dataReceiveLengthList[i]
                    if timecurrent - t > 1.0:
                        tIndex  = i
                        break
                t = self.dataReceiveTimeList[tIndex]
                if timecurrent - t != 0 :
                    instSpeed = float(oneSecRec)/(timecurrent - t)/1024.0
                else:
                    instSpeed = 0.0
                
                if(len(self.dataReceiveLengthList)>self.dataReceiveLengthListMaxLengh):
                    self.dataReceiveLengthList.pop(0)
                    self.dataReceiveTimeList.pop(0)
            # allspeed
            self.totalData = self.totalData + dataInWaitingSize
            deltaT =  timeNow-self.timetag
            if deltaT != 0 :
                allSpeed = float(self.totalData)/deltaT/1024.0
            else:
                allSpeed = 0.0
            thisupdateTime = time.time()
            if thisupdateTime - self.lastupdateSpeedTime > 0.2:
                self.speedupdate.emit([instSpeed,allSpeed,self.totalData])
                self.lastupdateSpeedTime = thisupdateTime

            # mavlink Speed calculation
            if self.needParseMavlink:
                # add new receive time
                if mavlinkMsgs is not None:
                    self.mavlinkManager.push(mavlinkMsgs) 

                mavlinkPkgSpeed=dict()
                self.mavlinkPkgSpeed_of_sys= dict()
                self.mavlinkBytesSpeed_of_sys = dict()
                for sysID, rectime_for_each_sys in self.mavlinkManager.dataReceiveTimeList.iteritems():
                    pkgCount = 0
                    bytesCount = 0 
                    mavlinkPkgSpeed_for_each_sys=dict()
                    for msgID, rectime in rectime_for_each_sys.iteritems():
                        msgsCount = 0 
                        for i in range(len(rectime)-1,-1,-1):
                            if timeNow - rectime[i]  < 5 :
                                if timeNow - rectime[i]  < 0 :
                                    Warning('Potential problem exists')
                                msgsCount += 1 
                                pkgCount  += 1
                                bytesCount+= self.mavlinkManager.dataReceiveLenList[sysID][msgID][i]
                            else:
                                break
                        mavlinkPkgSpeed_for_each_sys[msgID]=float(msgsCount)/5.0
                    
                    self.mavlinkPkgSpeed_of_sys[sysID] =  pkgCount /5.0
                    self.mavlinkBytesSpeed_of_sys[sysID] = bytesCount/5.0
                    mavlinkPkgSpeed[sysID] = mavlinkPkgSpeed_for_each_sys
                self.mavlinkSpeedUpdate.emit([self.mavlinkManager.dataReceiveTimeList,mavlinkPkgSpeed,self.mavlinkPkgSpeed_of_sys,self.mavlinkBytesSpeed_of_sys,
                self.mavlinkManager.dataReceiveCount_of_sysID,self.mavlinkManager.pkgReceiveCount_of_sysID])

    
    def swithMavlinkMode(self,needParseMavlink):
        #assert(type(needParseMavlink) is bool)
        self.needParseMavlink = needParseMavlink
        if needParseMavlink:
            #print "I need parse mavlink"
            if  self.mavlinkParser  is None:
                self.mavlinkParser=mavlinkParser()
                self.mavlinkManager= mavlinkDataManager.mavlinkDataManager()
                self.mavlinkManager.startTimer()
            else:
                self.mavlinkParser.clear()
                self.mavlinkManager.clear()
                self.mavlinkManager.startTimer()
        else:
            # need clear ?
            pass